
#pragma once

#include "BVH.h"
#include "Ray.h"

class MeshToTree
{
private:
	class Triangle : public BVH<Triangle>
	{
	private:
		RayTriangleIntersectionData mIntersectionData;
		PrecisionVector3<double> mPointInTri;
		AABB mAABB;
	public:
		Triangle(const Ogre::Vector3 &p1, const Ogre::Vector3 &p2, const Ogre::Vector3 &p3)
		{
			mPointInTri = PrecisionVector3<double>(p1.x, p1.y, p1.z);
			mAABB = AABB(p1, p2, p3);

			// use maximum precision (double) to compute the triangle normal and ray intersection parameters

			PrecisionVector3<double> dP1(p1.x, p1.y, p1.z);
			PrecisionVector3<double> dP2(p2.x, p2.y, p2.z);
			PrecisionVector3<double> dP3(p3.x, p3.y, p3.z);

			PrecisionVector3<double> v0 = dP2 - dP1;
			PrecisionVector3<double> v1 = dP3 - dP1;
			mIntersectionData.normal = v0.crossProduct(v1);
			mIntersectionData.normal.normalise();

			// slightly inspired by http://software.intel.com/en-us/articles/interactive-ray-tracing
			// determine dominant axis
			int domAxis = 0;
			if (std::abs(mIntersectionData.normal.y) >= std::abs(mIntersectionData.normal.x) && std::abs(mIntersectionData.normal.y) >= std::abs(mIntersectionData.normal.z))
				domAxis = 1;
			else if (std::abs(mIntersectionData.normal.z) >= std::abs(mIntersectionData.normal.x) && std::abs(mIntersectionData.normal.z) >= std::abs(mIntersectionData.normal.y))
				domAxis = 2;
			mIntersectionData.equationIndex1 = (domAxis+1) % 3;
			mIntersectionData.equationIndex2 = (domAxis+2) % 3;

			double a = p2[mIntersectionData.equationIndex1]-p1[mIntersectionData.equationIndex1];
			double b = p3[mIntersectionData.equationIndex1]-p1[mIntersectionData.equationIndex1];
			double c = p2[mIntersectionData.equationIndex2]-p1[mIntersectionData.equationIndex2];
			double d = p3[mIntersectionData.equationIndex2]-p1[mIntersectionData.equationIndex2];
			double denom = a*d - b*c;
			if (std::abs(denom) < std::numeric_limits<double>::epsilon())
			{
				//std::cout << "Degenerated triangle: " << p1 << " " << p2 << " " << p3 << " denom: " << denom << std::endl;
			}
			mIntersectionData.aInv = d/denom;
			mIntersectionData.bInv = -b/denom;
			mIntersectionData.cInv = -c/denom;
			mIntersectionData.dInv = a/denom;
		}
		const Triangle* rayIntersectClosest(Ray::Intersection &intersection, const Ray &ray) const override
		{
			if (ray.intersectTriangleBothSides(intersection,
				mPointInTri,
				mIntersectionData))
				return this;
			return nullptr;
		}

		__forceinline AABB getAABB() const
		{
			return mAABB;
		}
	};
	static bool compareIntersections(const std::pair<const Triangle*, Ray::Intersection> &int1, const std::pair<const Triangle*, Ray::Intersection> &int2)
	{
		return int1.second.t < int2.second.t;
	}

	class RayAABBIntersectionTestX : public Ray
	{
	public:
		RayAABBIntersectionTestX(const Ogre::Vector3 &_origin, const Ogre::Vector3 &_direction) : Ray(_origin, _direction) {}
		bool intersectAABB(const Ogre::Vector3 bounds[2], float tNear, float tFar) const override
		{
			float tEnter = bounds[0].x - this->origin.x;
			float tExit = bounds[1].x - this->origin.x;
			return (bounds[0].z <= this->origin.z && bounds[1].z >= this->origin.z
				&& bounds[0].y <= this->origin.y && bounds[1].y >= this->origin.y
				&& tEnter < tFar && tExit > tNear);
		}

		bool intersectAABB(const Ogre::Vector3 bounds[2]) const override
		{
			return (bounds[0].z <= this->origin.z && bounds[1].z >= this->origin.z
				&& bounds[0].y <= this->origin.y && bounds[1].y >= this->origin.y);
		}
	};

	class RayAABBIntersectionTestY : public Ray
	{
	public:
		RayAABBIntersectionTestY(const Ogre::Vector3 &_origin, const Ogre::Vector3 &_direction) : Ray(_origin, _direction) {}
		bool intersectAABB(const Ogre::Vector3 bounds[2], float tNear, float tFar) const override
		{
			float tEnter = bounds[0].y - this->origin.y;
			float tExit = bounds[1].y - this->origin.y;
			return (bounds[0].z <= this->origin.z && bounds[1].z >= this->origin.z
				&& bounds[0].x <= this->origin.x && bounds[1].x >= this->origin.x
				&& tEnter < tFar && tExit > tNear);
		}

		bool intersectAABB(const Ogre::Vector3 bounds[2]) const override
		{
			return (bounds[0].z <= this->origin.z && bounds[1].z >= this->origin.z
				&& bounds[0].x <= this->origin.x && bounds[1].x >= this->origin.x);
		}
	};

	class RayAABBIntersectionTestZ : public Ray
	{
	public:
		RayAABBIntersectionTestZ(const Ogre::Vector3 &_origin, const Ogre::Vector3 &_direction) : Ray(_origin, _direction) {}
		bool intersectAABB(const Ogre::Vector3 bounds[2], float tNear, float tFar) const override
		{
			float tEnter = bounds[0].z - this->origin.z;
			float tExit = bounds[1].z - this->origin.z;
			return (bounds[0].x <= this->origin.x && bounds[1].x >= this->origin.x
				&& bounds[0].y <= this->origin.y && bounds[1].y >= this->origin.y
				&& tEnter < tFar && tExit > tNear);
		}

		bool intersectAABB(const Ogre::Vector3 bounds[2]) const override
		{
			return (bounds[0].x <= this->origin.x && bounds[1].x >= this->origin.x
				&& bounds[0].y <= this->origin.y && bounds[1].y >= this->origin.y);
		}
	};

public:

	static void processRayResult(const Ogre::Vector3 &rayOrigin, double rayDirZ, int x, int y, const Vector3i &min, const Vector3i &axis, int voxelVoteSize, unsigned char *voxelVotes, std::vector<std::pair<const Triangle*, Ray::Intersection>> &intersections)
	{
		if (intersections.size() < 2) return;

		PrecisionVector3<double> rayOriginD = PrecisionVector3<double>(rayOrigin.x, rayOrigin.y, rayOrigin.z);

		std::sort(intersections.begin(), intersections.end(), compareIntersections);
		for (auto i = intersections.begin(); i != intersections.end()-1; ++i)
		{
			for (auto i2 = i+1; i2 != intersections.end(); ++i2)
			{
				const double minHitDistDiff = 0.000001;
				if (std::abs(i->second.t - i2->second.t) < minHitDistDiff)
				{
					if (i->second.flags == i2->second.flags)
					{	// tris are of the same type - merge
						unsigned int iIndex = (unsigned int)std::distance(intersections.begin(), i);
						i2 = intersections.erase(i2);
						i = intersections.begin() + iIndex;
						if (i2 == intersections.end()) break;
					}
				}
			}
			if (intersections.empty() || i == intersections.end()-1) break;
		}

		int insideCounter = 0;
		double enter = 0.0;
		for (auto i = intersections.begin(); i != intersections.end(); i++)
		{
			bool wasOutside = insideCounter <= 0;
			if (i->second.flags == 0) insideCounter++;
			else insideCounter--;
			if (insideCounter < 0)
			{
				// std::cout << "Warning: Mesh is not closed!" << std::endl;
				insideCounter = 0;
				//break;
			}
			if (wasOutside && insideCounter > 0) enter = rayOriginD[axis.z] + (i->second.t * rayDirZ);
			else if (!wasOutside && insideCounter <= 0)
			{
				double exit = rayOriginD[axis.z] + (i->second.t * rayDirZ);
				if (enter > exit) std::swap(enter, exit);
				for (int z = static_cast<int>(enter+0.5); z < static_cast<int>(exit+0.5); z++)
				{
					Vector3i voxelPos;
					voxelPos[axis.x] = x;
					voxelPos[axis.y] = y;
					voxelPos[axis.z] = z;
					voxelPos = voxelPos - min;
					int index = voxelPos.x*voxelVoteSize*voxelVoteSize + voxelPos.y*voxelVoteSize + voxelPos.z;
					voxelVotes[index]++;
				}
			}
		}
	}

	template<class SpecialRay>
	static void sampleSide(unsigned char *voxelVotes, int voxelVoteSize, BVHNodeThreaded<Triangle> *bvhNode, int normalDir)
	{
		Vector3i axis((normalDir+1) % 3, (normalDir+2) % 3, normalDir);
		Ogre::Vector3 rayDir(0,0,0);
		rayDir[normalDir] = 1.0f;
		Ogre::Vector3 constantOffset(Ogre::Math::PI * 0.01f, Ogre::Math::PI * 0.01f, Ogre::Math::PI * 0.01f);
		Ogre::Vector3 rayOrigin;
		int xMin = static_cast<int>(bvhNode->getAABB().min.x);
		int yMin = static_cast<int>(bvhNode->getAABB().min.y);
		int zMin = static_cast<int>(bvhNode->getAABB().min.z);
		Vector3i min(xMin, yMin, zMin);
		for (int x = static_cast<int>(bvhNode->getAABB().min[axis.x]); x < static_cast<int>(bvhNode->getAABB().max[axis.x]); x++)
		{
			rayOrigin[axis.x] = (Ogre::Real)x + 0.5f;
			for (int y = static_cast<int>(bvhNode->getAABB().min[axis.y]); y < static_cast<int>(bvhNode->getAABB().max[axis.y]); y++)
			{
				rayOrigin[axis.y] = (Ogre::Real)y + 0.5f;
				rayOrigin[axis.z] = bvhNode->getAABB().min[axis.z] - 0.1f;
				SpecialRay ray(rayOrigin + constantOffset, rayDir);
				std::vector<std::pair<const Triangle*, Ray::Intersection>> intersections;
				bvhNode->rayIntersectAll(ray, intersections);
				processRayResult(rayOrigin, 1.0f, x, y, min, axis, voxelVoteSize, voxelVotes, intersections);

				/*intersections.clear();
				rayOrigin[axis.z] = bvhNode->getAABB().max[axis.z] + 0.1f;
				Ogre::Vector3 rayDirInv = rayDir;
				rayDirInv[axis.z] = -1.0f;
				SpecialRay rayInv(rayOrigin + constantOffset, rayDirInv);
				bvhNode->rayIntersectAll(rayInv, intersections);
				processRayResult(rayOrigin, -1.0f, x, y, min, axis, voxelVoteSize, voxelVotes, intersections);*/

				/*for (int i = 0; i < 4; i++)
				{
					Ogre::Vector3 offset(0,0,0);
					offset[axis.x] = 0.25f;
					offset[axis.y] = 0.25f;
					if (i % 2 == 1) offset[axis.y] = -0.25f;
					if (i >= 2) offset[axis.x] = -0.25f;
					SpecialRay ray2(rayOrigin + offset + constantOffset, rayDir);
					std::vector<std::pair<const Triangle*, Ray::Intersection>> intersections;
					bvhNode->rayIntersectAll(ray2, intersections);
					processRayResult(rayOrigin, 1.0f, x, y, min, axis, voxelVoteSize, voxelVotes, intersections);
				}*/
			}
		}
	}
	template<class T>
	static void insertMeshVolumeIntoTree(NTree<Vector3i, T> &tree, const std::vector<Ogre::Vector3> &vertexPositions, const std::vector<unsigned int> &indices, float voxelPerUnit)
	{
		std::vector<Triangle*> triangles;
		for (auto i = indices.begin(); i != indices.end(); i+=3)
			triangles.push_back(new Triangle(vertexPositions[*i]*voxelPerUnit, vertexPositions[*(i+1)]*voxelPerUnit, vertexPositions[*(i+2)]*voxelPerUnit));
		std::cout << "[Mesh to Volume] Building BVH..." << std::endl;
		BVHNodeThreaded<Triangle> *bvhNode = new BVHNodeThreaded<Triangle>(triangles, 0, (int)triangles.size(), 0, 4);
		float max = std::max(std::max(bvhNode->getAABB().max.x, bvhNode->getAABB().max.y), bvhNode->getAABB().max.z);
		float min = std::min(std::min(bvhNode->getAABB().min.x, bvhNode->getAABB().min.y), bvhNode->getAABB().min.z);
		int voxelSize = (int)(std::ceil(max-min));
		Exponent expo = AuxMath::log2(voxelSize);
		tree.raise(expo+1);

		unsigned char *voxelVotes = new unsigned char[voxelSize*voxelSize*voxelSize];
		if (!voxelVotes)
		{
			std::cout << "[Mesh to Volume] Error: Could not allocate voting array!" << std::endl;
			return;
		}
		for (int i = 0; i < voxelSize*voxelSize*voxelSize; i++) voxelVotes[i] = 0;
		std::cout << "[Mesh to Volume] Sampling side 1..." << std::endl;
		sampleSide<RayAABBIntersectionTestX>(voxelVotes, voxelSize, bvhNode, 0);
		std::cout << "[Mesh to Volume] Sampling side 2..." << std::endl;
		sampleSide<RayAABBIntersectionTestY>(voxelVotes, voxelSize, bvhNode, 1);
		std::cout << "[Mesh to Volume] Sampling side 3..." << std::endl;
		sampleSide<RayAABBIntersectionTestZ>(voxelVotes, voxelSize, bvhNode, 2);
		std::cout << "[Mesh to Volume] Building tree..." << std::endl;

		int xMin = (int)std::floor(bvhNode->getAABB().min.x);
		int yMin = (int)std::floor(bvhNode->getAABB().min.y);
		int zMin = (int)std::floor(bvhNode->getAABB().min.z);

		for (int x = 0; x < voxelSize; x++)
			for (int y = 0; y < voxelSize; y++)
				for (int z = 0; z < voxelSize; z++)
					if (voxelVotes[x*voxelSize*voxelSize + y*voxelSize + z] > 1)
						tree.insertVoxel(Area<Vector3i>(0, Vector3i(x+xMin, y+yMin, z+zMin)), T());
		tree.simplify();

		std::cout << "[Mesh to Volume] Cleaning up..." << std::endl;
		delete[] voxelVotes;

		delete bvhNode;
		for (auto i = triangles.begin(); i != triangles.end(); i++)
			delete *i;
		std::cout << "[Mesh to Volume] Done!" << std::endl;
	}
};
